#include "map.h"
#include <QDebug>

Map::Map()
{
    map.create(400, 400, CV_8UC3);
    scale=0.05;
    centx=200;
    centy=200;
    map.at<Vec3b>(10, 10) = Vec3b(0,0, 255);

    Mat zmmap=map;
    resize(map,zmmap,Size(map.cols*2,map.rows*2));
    imshow("MAP",zmmap);

}
void Map::clear()
{
    map=Mat::ones(400, 400, CV_8UC3);
}
void Map::update(double x,double y,double heading, QVector<QPointF> senarray)
{
    double tx,ty;
    double ang;
    for(int i=0;i<senarray.size();i++)
    {
        ang=senarray[i].x()+heading;
        ang=ang>=360?ang-360:ang;
        ang=ang<0?ang+360:ang;


        tx=centx-x/scale-senarray[i].y()*0.01*sin(-ang/180*CV_PI)/scale;
        ty=centy-y/scale-senarray[i].y()*0.01*cos(-ang/180*CV_PI)/scale;
        map.at<Vec3b>(int(ty), int(tx)) = Vec3b(0,0, 255);
//        Mat imap = map.clone();
//        line(imap,Point(int(ty),int(tx)),Point(centy-y/scale,centx-x/scale),Scalar(255,0,255));
//        imshow("iMAP",imap);

    }
    Mat zmmap=map;
    resize(map,zmmap,Size(map.cols*2,map.rows*2),0,0,INTER_LINEAR);
    //map1=zmmap;
    //map.at<Vec3b>(centy-y/scale,centx-x*2/scale) = Vec3b(0,255, 0);
    map.at<Vec3b>(centy-y/scale,centx-x/scale) = Vec3b(0,255, 0);
    Mat vmap=zmmap.clone();
    showmap = vmap.clone();
    circle(vmap,Point(centx*2-x*2/scale,centy*2-y*2/scale),10,Scalar(255,0,255));
    imshow("MAP",vmap);
}

